#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Int32.h"
#include "std_msgs/Empty.h"
#include "tf/transform_datatypes.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/PoseStamped.h"
#include "tello_baotou/Position.h"
#include <math.h>
#include <iostream>
#define PI 3.1415926
using namespace std;

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "test1");
	ros::NodeHandle nh("");

    geometry_msgs::PoseStamped pos1,pos2,pos3,pos4;
    pos1.pose.position.x = 3;
    pos2.pose.position.x = -1;
    pos3.pose.position.x = -1;
    pos4.pose.position.x = 1;

    pos1.pose.position.y = 1;
    pos2.pose.position.y = 1;
    pos3.pose.position.y = -1;
    pos4.pose.position.y = -1;

    ros::Publisher pub1 = nh.advertise<geometry_msgs::PointStamped>("/tello20/pose", 10);
    ros::Publisher pub2 = nh.advertise<geometry_msgs::PointStamped>("/tello21/pose", 10);
    ros::Publisher pub3 = nh.advertise<geometry_msgs::PointStamped>("/tello22/pose", 10);
    ros::Publisher pub4 = nh.advertise<geometry_msgs::PointStamped>("/tello24/pose", 10);
    ros::Rate loop_rate(100);
    while (ros::ok())
    {
        double t = ros::Time::now().toSec();
        cout<<pos1.pose.position.x<<endl;
        pub1.publish(pos1);
        pub2.publish(pos2);
        pub3.publish(pos3);
        pub4.publish(pos4);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}
